#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from visualization_msgs.msg import InteractiveMarkerFeedback

class InteractiveMarkerFeedbackSubscriber(Node):

    def __init__(self):
        super().__init__('interactive_marker_feedback_subscriber')
        self.subscription = self.create_subscription(
            InteractiveMarkerFeedback,
            '/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback',
            self.feedback_callback,
            10)
        self.subscription  # prevent unused variable warning

    def feedback_callback(self, msg):
        # Print the position of the interactive marker
        position = msg.pose.position
        orientation = msg.pose.orientation
        self.get_logger().info(
            f"Position: x={position.x}, y={position.y}, z={position.z}\n"
            f"Orientation: x={orientation.x}, y={orientation.y}, z={orientation.z}, w={orientation.w}"
        )

def main(args=None):
    rclpy.init(args=args)
    node = InteractiveMarkerFeedbackSubscriber()
    rclpy.spin(node)
    rclpy.shutdown()

if __name__ == '__main__':
    main()
